import numpy
cimport numpy


def shrink_mrf1_icm(numpy.ndarray[numpy.float64_t,ndim=1] vec,
			double beta, double tau, double converge=1e-6):
	cdef numpy.ndarray[numpy.float64_t,ndim=1] vec1 = vec.copy()
	cdef numpy.ndarray[numpy.float64_t,ndim=1] old_vec = vec.copy()

	cdef int N = len(vec)

	cdef Py_ssize_t i
	cdef double x

	cdef prec1 = 1.0/(tau + abs(beta))
	cdef prec2 = 1.0/(tau + 2.0*abs(beta))

	count = 0
	while 1:
		old_vec = vec1.copy()

		x = beta*vec1[1] + tau*vec[0]
		vec1[0] = x*prec1

		for i from 0 < i < N-1:
			x = beta*(vec1[i-1] + vec1[i+1]) + tau*vec[i]
			vec1[i] = x*prec2

		x = beta*vec1[N-2] + tau*vec[N-1]
		vec1[N-1] = x*prec1

		diff_max = abs(vec1 - old_vec).max()
		
		if diff_max < converge: break
		count += 1

	return vec1


#def shrink_mrf2_icm(numpy.ndarray[numpy.float64_t,ndim=2] obs,
#			numpy.ndarray[numpy.float64_t,ndim=2] beta,
#			double tau, double converge=1e-6):
#
#	cdef Py_ssize_t N = obs.shape[0]
#	cdef Py_ssize_t P = obs.shape[1]
#	cdef numpy.ndarray[numpy.float64_t,ndim=2] ttr = obs.copy()
#	cdef numpy.ndarray[numpy.float64_t,ndim=2] old_ttr = obs.copy()
#
#	cdef Py_ssize_t i, j
#	cdef double prec, x
#
#	
#	while 1:
#		old_ttr = ttr.copy()
#
#		# top left corner
#		prec = 1.0/(tau + abs(beta[1,2]) + abs(beta[2,1]) + abs(beta[2,2]))
#		x = beta[1,2]*ttr[0,1] + beta[2,1]*ttr[1,0] + beta[2,2]*ttr[1,1]
#		x += tau*obs[0,0]
#		ttr[0,0] = x*prec
#
#		# top right corner
#		prec = 1.0/(tau + abs(beta[1,0]) + abs(beta[2,0]) + abs(beta[2,1]))
#		x = beta[1,0]*ttr[0,P-2] + beta[2,0]*ttr[1,P-2] + beta[2,2]*ttr[1,P-1]
#		x += tau*obs[0,P-1]
#		ttr[0,P-1] = x*prec
#
#		# bottom left corner
#		prec = 1.0/(tau + abs(beta[0,1]) + abs(beta[0,2]) + abs(beta[1,2]))
#		x = beta[0,1]*ttr[N-2,0] + beta[0,2]*ttr[N-2,1] + beta[1,2]*ttr[N-1,1]
#		x += tau*obs[N-1,0]
#		ttr[N-1,0] = x*prec
#
#		# bottom right corner
#		prec = 1.0/(tau + abs(beta[0,0]) + abs(beta[0,1]) + abs(beta[1,0]))
#		x = beta[0,0]*ttr[N-2,P-2] + beta[0,1]*ttr[N-2,P-1]
#		x += beta[1,0]*ttr[N-1,P-2]
#		x += tau*obs[N-1,P-1]
#		ttr[N-1,P-1] = x*prec
#
#
#		# top side
#		prec = tau + abs(beta[1,0]) + abs(beta[1,2]) + abs(beta[2,0])
#		prec += abs(beta[2,1]) + abs(beta[2,2])
#		prec = 1.0/prec
#
#		for j from 0 < j < P-1:
#			x = beta[1,0]*ttr[0,j-1] + beta[1,2]*ttr[0,j+1]
#			x += beta[2,0]*ttr[1,j-1] + beta[2,1]*ttr[1,j]
#			x += beta[2,2]*ttr[1,j+1] + tau*obs[0,j]
#			ttr[0,j] = x*prec
#
#		# bottom side
#		prec = tau + abs(beta[0,0]) + abs(beta[0,1]) + abs(beta[0,2])
#		prec += abs(beta[1,0]) + abs(beta[1,2])
#		prec = 1.0/prec
#
#		for j from 0 < j < P-1:
#			x = beta[0,0]*ttr[N-2,j-1] + beta[0,1]*ttr[N-2,j]
#			x += beta[0,2]*ttr[N-2,j+1] + beta[1,0]*ttr[N-1,j-1]
#			x += beta[1,2]*ttr[N-1,j+1] + tau*obs[N-1,j]
#			ttr[N-1,j] = x*prec
#
#
#		# left side
#		prec = tau + abs(beta[0,1]) + abs(beta[0,2]) + abs(beta[1,2])
#		prec += abs(beta[2,1]) + abs(beta[2,2])
#		prec = 1.0/prec
#
#		for i from 0 < i < N-1:
#			x = beta[0,1]*ttr[i-1,0] + beta[0,2]*ttr[i-1,1]
#			x += beta[1,2]*ttr[i,1] + beta[2,1]*ttr[i+1,0]
#			x += beta[2,2]*ttr[i+1,1] + tau*obs[i,0]
#			ttr[i,0] = x*prec
#
#
#		# right side
#		prec = tau + abs(beta[0,0]) + abs(beta[0,1]) + abs(beta[1,0])
#		prec += abs(beta[2,0]) + abs(beta[2,1])
#		prec = 1.0/prec
#
#		for i from 0 < i < N-1:
#			x = beta[0,0]*ttr[i-1,P-2] + beta[0,1]*ttr[i-1,P-1]
#			x += beta[1,0]*ttr[i,P-2] + beta[2,0]*ttr[i+1,P-2]
#			x += beta[2,1]*ttr[i+1,P-1] + tau*obs[i,P-1]
#			ttr[i,P-1] = x*prec
#
#		# middle
#		prec = (abs(beta)).sum() + tau
#		prec = 1.0/prec
#
#
#		for i from 0 < i < N-1:
#			for j from 0 < j < P-1:
#				x = beta[0,0]*ttr[i-1,j-1] + beta[0,1]*ttr[i-1,j]
#				x += beta[0,2]*ttr[i-1,j+1] + beta[1,0]*ttr[i,j-1]
#				x += beta[1,2]*ttr[i,j+1] + beta[2,0]*ttr[i+1,j-1]
#				x += beta[2,1]*ttr[i+1,j] + beta[2,2]*ttr[i+1,j+1]
#				x += tau*obs[i,j]
#				ttr[i,j] = x*prec
#
#		diff_max = abs(ttr - old_ttr).max()
#		
#		if diff_max < converge: break
#
#	return ttr
